## Copyright (C) 2006 EPRI
## by Tom Short, tshort@epri.com
##
## This program is free software; you can redistribute it and/or modify
## it under the terms of the GNU General Public License as published by
## the Free Software Foundation; either version 2 of the License, or
## (at your option) any later version.
##
## This program is distributed in the hope that it will be useful,
## but WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
## GNU General Public License for more details.
##
## You should have received a copy of the GNU General Public License
## along with this program; if not, write to the Free Software
## Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#for (filename in dir(pattern=".*R$")) {cat(filename);source(filename)}

Filter <- function(filt, ...) UseMethod("Filter")

# Octave/Matlab-compatible filter function
# y = Filter (b, a, x)
Filter.default <- function(filt, a, x, ...) {
  if (length(filt))
    if (is.array(x))
      x <- na.omit(stats:::filter(rbind(array(0,dim=c(length(filt) - 1),ncol(x)), x), filt / a[1] , sides = 1))
    else
      x <- na.omit(stats:::filter(c(rep(0,length(filt) - 1), x), filt / a[1] , sides = 1))
  if (length(a) > 1)
    x <- stats:::filter(x, -a[-1] / a[1], method = "recursive")
  x
}

Filter.Arma <- function(filt, x, ...) # IIR
  Filter(filt$b, filt$a, x)

Filter.Ma <- function(filt, x, ...) # FIR
  Filter(unclass(filt), 1, x)

Filter.Zpg <- function(filt, x, ...) # zero-pole-gain form
  Filter(as.Arma(filt), x)

MedianFilter <- function(n = 3) {
  res <- list(n = n)
  class(res) <- "MedianFilter"
  res
}

Filter.MedianFilter <- function(filt, x, ...) {
  if (NCOL(x) > 1)
    apply(x, 2, function(x) runmed(x, filt$n, ...))
  else
    runmed(x, filt$n, ...)
}

medfilt1 <- function(x, n = 3, ...) {
  if (NCOL(x) > 1)
    apply(x, 2, function(x) runmed(x, n, ...))
  else
    runmed(x, n, ...)
}

spencerFilter <- function() {
  Ma(c(-3, -6, -5, 3, 21, 46, 67, 74, 67, 46, 21, 3, -5, -6, -3) / 320)
}

spencer  <- function(x)  {
  fftfilt(c(-3, -6, -5, 3, 21, 46, 67, 74, 67, 46, 21, 3, -5, -6, -3) / 320,
          x)
}

FilterOfOrder <- function(n, Wc, type, ...) {
  res = list(n = n, Wc = Wc, type = type, ...)
  class(res) = "FilterOfOrder"
  res
}

an <- function(degrees) {
  exp(1i*degrees*pi/180)
}

roots <- function(x) {
  polyroot(rev(x))
}

Arma <- function(b, a) {
  res <- list(b = b, a = a)
  class(res) <- "Arma"
  res
}

as.Arma <- function(x, ...) UseMethod("as.Arma")

as.Arma.Arma <- function(x, ...) x

Zpg <- function(zero, pole, gain) {
  res <- list(zero = zero, pole = pole, gain = gain)
  class(res) <- "Zpg"
  res
}

as.Zpg <- function(x, ...) UseMethod("as.Zpg")

as.Zpg.Zpg <- function(x, ...) x

as.Zpg.Arma <- function(x, ...) {
  Zpg(pole = roots(x$a), zero = roots(x$b), gain = x$b[1] / x$a[1])
}

as.Zpg.Ma <- function(x, ...) {
  as.Zpg(as.Arma(x))
}

as.Arma.Zpg <- function(x, ...) {
  b = Re(x$gain*Poly(x$zero))
  a = Re(Poly(x$pole))
  Arma(b = b, a = a)
}

as.Arma.Ma <- function(x, ...) {
  Arma(b = unclass(x), a = 1)
}

Ma <- function(b) {
  class(b) <- "Ma"
  b
}

polyval <- function(coef, z) {
#           n             n-1
#  coef[1]*z   + coef[2]*z   + ... + coef[n]
  n = length(coef)
  nz = length(z)
  as.vector(matrix(z, nz, n) ^ t(matrix((n-1):0, n, nz)) %*% coef)
}

conv <- function(x, y) {
  n <- length(x) + length(y) - 1
  res <- ifft(fft(postpad(x, n)) * fft(postpad(y, n)))
  if (is.numeric(x) && is.numeric(y))
    res <- Re(res)
  res
}

xcorr <- function(x, y) {
    nrx <-      NROW(x)
    nr <-       nrx + NROW(y) - 1
    nc <-       max(NCOL(x), NCOL(y))
    isArray <-  is.array(x) || is.array(y)

    ## cross-correlation computed in the frequency domain
    res <-      as.vector( Fft(x,nr) ) * Conj( as.vector( Fft(y,nr) ) )
    if (isArray)
        dim(res) <- c(nr,nc)
    res <-      ifft( res )
    if (is.numeric(x) && is.numeric(y))
        res <-  Re(res)

    ## move negative lags to be first
    if (isArray)
        res <-  rbind(res[(nrx+1):nr,,drop=FALSE], res[1:nrx,,drop=FALSE])
    else
        res <-  c(res[(nrx+1):nr], res[1:nrx])
    res
}

localSums <- function(x, n) {
    nr <-       NROW(x) + 2*n - 1
    nc <-       NCOL(x)

    if (is.array(x)) {
        B <-    rbind(matrix(0,n,nc), x, matrix(0,n-1,nc))
        s <-    matrix(0,nr,nc)
        for (ic in 1:nc)
            s[,ic] <-   cumsum(B[,ic])
        S <-    s[-(1:n),,drop=FALSE] - s[1:(nr-n),,drop=FALSE]
    } else {
        B <-    c(rep(0,n), x, rep(0,n-1))
        s <-    cumsum(B)
        S <-    s[-(1:n)] - s[1:(nr-n)]
    }
    S
}

normxcorr <- function(x, T) {
    nrx <-      NROW(x)
    nrT <-      NROW(T)
    ncx <-      NCOL(x)
    ncT <-      NCOL(T)
    nr <-       nrx + nrT - 1
    nc <-       max(ncx, ncT)
    if (nrT > nrx)
        stop("The size of 'T' should be smaller or equal to the size of 'x'")

    ## make T zero-mean, compute column sums of T square
    if (is.array(T)) {
        T <-    t( t(T) - colMeans(T) )
        smT2 <- colSums(T^2)
    } else {
        T <-    T - mean(T)
        smT2 <- sum(T^2)
    }

    ## compute local sums for the denominator
    lsm <-      localSums(x,   nrT)
    lsm2 <-     localSums(x^2, nrT)

    ## compute numerator and denominator
    num <-      xcorr(x, T)
    denx <-     lsm2 - (lsm^2) / nrT
    recycle <-  sign(ncx - ncT)     # +1: recycle T, -1: recycle x, 0: no recycling
    if (recycle == 1)
        smT2 <- rep(smT2, nc/ncT)
    else if (recycle == -1)
        denx <- matrix(denx, nr, nc)
    res <-      num / sqrt(t( t(denx) * smT2 ))
    res
}

postpad <- function(x, n, val = 0) {
  nx <- NROW(x)
  if (n > nx)
    if (is.array(x))
      rbind(x, array(val, dim = c(n - nx, NCOL(x))))
    else
      c(x, rep(val, n - nx))
  else
    if (is.array(x))
      x[1:n,,drop=FALSE]
    else
      x[1:n]
}

ifft <- function(x, n = NROW(x))
  if (is.array(x)) {
    mvfft(postpad(x, n), inverse = TRUE) / n
  } else {
    fft(postpad(x, n), inverse = TRUE) / n
  }

Fft <- function(x, n = NROW(x))
  if (is.array(x)) {
    mvfft(postpad(x, n))
  } else {
    fft(postpad(x, n))
  }

eps <- .Machine$double.eps

sinc <- function(x) sin(pi*x)/(pi*x)

linspace <- function(from, to, n = 500)
  seq(from, to, length = n)

logseq <- function(from, to, n = 500)
  exp(seq(log(abs(from)), log(abs(to)), length = n))

roots <- function(x) {
  polyroot(rev(x))
}
